#ifndef _BUOLA_CV_REGISTRATION_CPOSEGRAPH_H_
#define _BUOLA_CV_REGISTRATION_CPOSEGRAPH_H_

#include <buola/mat.h>
#include <buola/mat/transformations.h>
#include <map>
#include <set>

namespace buola { namespace cvi {

class CPoseGraph
{
public:
    struct SVertex;
    
    struct SEdge
    {
        SVertex *m1;                    ///<first vertex
        SVertex *m2;                    ///<second vertex
        SVertex *mTop;                  ///<the vertex with the smalles level
        int mLength;                    ///<length of the path on the tre (number of vertices involved)
        mat::C3DRotTrans_d mTransform;  ///<transformation describing the constraint
        mat::CMat<double,6,6> mInfo;
        ///\todo there should be something like information
        bool mMark;
        double mLearningRate;
    };

    struct SVertex 
    {
        //graph-related elements
        int mID;                        ///<id of the vertex in the graph
        std::vector<SEdge*> mEdges;
        
        //tree-related elements
        int mLevel;
        SVertex *mParent;
        SEdge *mParentEdge;
        std::vector<SEdge*> mChildren;
        
        ///\todo still don't know what's the difference between the two
        mat::C3DRotTrans_d mTransform;
        mat::C3DRotTrans_d mPose;
        mat::C3DRotTrans_d mParameters;
        
        bool mMark;
    };

    CPoseGraph()
    {}

    virtual ~CPoseGraph();
    
    SVertex &Vertex(int pID)
    {
        auto lIt=mVertices.find(pID);
        if(lIt==mVertices.end()) throw XNotFound("can't find vertex in pose graph");
        return *lIt->second;
    }
    
    const SVertex &Vertex(int pID) const
    {
        auto lIt=mVertices.find(pID);
        if(lIt==mVertices.end()) throw XNotFound("can't find vertex in pose graph");
        return *lIt->second;
    }

    size_t EdgeCount() const    {   return mSortedEdges.size(); }
    size_t VertexCount() const  {   return mVertices.size();    }
    
    SEdge &Edge(int pID1,int pID2);
    const SEdge &Edge(int pID1,int pID2) const;
    
    SVertex &AddVertex(int pID,const mat::CMat34d &pPose);
    void RemoveVertex(int pID);
    
    SEdge &AddEdge(const SVertex &p1,const SVertex &p2,const mat::CMat34d &pTrans);//info missing
    void RemoveEdge(const SEdge &pEdge);
    
    SEdge &AddIncrementalEdge(const SVertex &p1,const SVertex &p2,const mat::CMat34d &pTrans);//info missing
    
    std::multiset<SEdge*> AffectedEdges(const SVertex &pV);
    std::multiset<SEdge*> AffectedEdges(const std::set<SVertex> &pV);
    
    template<typename tActor>
    void VisitBreadthFirst(tActor &&pActor);

    template<typename tActor>
    void VisitDepthFirst(tActor &&pActor);
    
    bool BuildTree(int pID);
    bool BuildSimpleTree();
    void RevertEdge(const SEdge &pEdge);

    virtual void RevertEdgeInfo(SEdge &pEdge)=0;
    virtual void InitializeFromParentEdge(SVertex &pVertex)=0;
    
    void Clear();
    
    void SortEdges();
    
    int MaxPathLength();
    int TotalPathLength();
    
    void CompressIndices();
    int MaxIndex();

    bool SanityCheck();

    const std::multiset<SEdge*> &SortedEdges() const {   return mSortedEdges;    }
    
private:
    void FillEdgeInfo(SEdge &pEdge);
    void FillEdgesInfo();
    
private:
    SVertex *mRoot;
    std::map<int,SVertex*> mVertices;
    std::multiset<SEdge*> mSortedEdges;
};

void optimize_tree(const CPoseGraph &pGraph);
    
/*namespace cvi*/ } /*namespace buola*/ }

#endif
